#include "sv2_camera_read.h"
#include <cmath>
#include <fstream>


namespace sv2 {

using namespace cv;

CameraType CameraCppNode::getCameraType(const std::string &camera_string)
{
  std::unordered_map<std::string, CameraType> cameraTypeMap = {
    {"NONE", CameraType::NONE},
    {"WEBCAM", CameraType::WEBCAM},
    {"V4L2CAM", CameraType::V4L2CAM},
    {"MIPI", CameraType::MIPI},
    {"RTSP", CameraType::RTSP},
    {"VIDEO", CameraType::VIDEO},
    {"G1", CameraType::G1},
    {"Q10", CameraType::Q10},
    {"GX40", CameraType::GX40},
    {"SU17", CameraType::SU17}
  };
  auto it = cameraTypeMap.find(camera_string);
  if (it != cameraTypeMap.end())
  {
    return it->second;
  }
  return CameraType::NONE;
}

void CameraCppNode::run()
{
  auto start_time = std::chrono::steady_clock::now();
  while (this->is_running())
  {
    this->cap->read(img_);
    nlohmann::json img_msg = sms::cvimg2sms(img_);

    auto current = std::chrono::steady_clock::now();
    std::chrono::duration<double> elapsed_seconds = current - start_time;
    if (elapsed_seconds.count() >= 1.0) {
      nlohmann::json calib_msg = sms::def_msg("sensor_msgs::CameraCalibration");
      calib_msg["type"] = "sensor_msgs::CameraCalibration";
      calib_msg["frame_id"] = this->frame_id;
      calib_msg["width"] = this->image_width;
      calib_msg["height"] = this->image_height;
      calib_msg["distortion_model"]= "plumb_bob";
      calib_msg["K"] = this->camera_matrix;
      calib_msg["D"] = this->distortion_coefficients;
      calib_msg["R"] = this->rectification;
      calib_msg["P"] = this->projection;

      // 发送sensor_msgs::CameraCalibration话题
      _calib_pub.publish(calib_msg);

      start_time = current;
    }

    // 发送sensor_msgs::CompressedImage话题
    _image_pub.publish(img_msg);

    // cv::imshow("image", image);
    // cv::waitKey(5);
  }
}

}
